#include <stdio.h>
#include <string.h>
#include <stdlib.h>
#include <unistd.h>
#include <pthread.h>
#include <signal.h>
#include <sys/socket.h>
#include <sys/types.h>
#include <netinet/in.h>
#include <time.h>
#include "include/filters.h"
#include "include/gpio.h"
#include "include/qfmode.h"

#define	BUFNUM     2
#define	FRAMEHEAD  10
#define	FRAMEEND   40

#define NETPORT	9025

char	*can_read_buf;
int	issending = 0;

unsigned short node_id;
unsigned short fs[] = {50, 10, 100, 280, 1000}; /* sample frequency */
unsigned short fs_type = 0; /* Default fs=50Hz */
unsigned char  channal_num = 3;
int	bufsize;

pthread_t pid_send;

void handler(int sig)
{
	if(sig == SIGPIPE) {
		/* Kill send pthread.
		   By default this signal kill the process.
		 */
		pthread_cancel(pid_send); 
	}
	else {
		;// Do nothing, just skip pause().
	}
}

void sendend()
{
	issending = 0;
}

void backtoconfig()
{
	sleep(1);
	qf_run_mode(0x04, 0x00, (unsigned char *)NULL, (unsigned short *)NULL);
	printf("Back to CONFIGURE mode.\n");
}

void *send_pthread(void *fd)
{
	int r = 0;
	int i=0;
	int client_fd = *(int *)fd;
	char *send_buf;

	send_buf = (char *)malloc(FRAMEHEAD + bufsize);

	signal(SIGPIPE, handler);
	signal(SIGUSR1, handler);
	pthread_setcancelstate(PTHREAD_CANCEL_ASYNCHRONOUS, NULL);

	pthread_cleanup_push(sendend, NULL);
	while(1) {
		pause();	//Go on when get signal SIGUSR1.

		if(can_read_buf != NULL) {
			sprintf(send_buf, "SDv2\0\0\0\0\0A");
			*(unsigned short *)(send_buf + 5) = fs[fs_type];
			*(send_buf +7) = channal_num;
			memcpy(send_buf+FRAMEHEAD, can_read_buf, bufsize);

			r = send(client_fd, send_buf, FRAMEHEAD+bufsize, 0);
			can_read_buf = NULL;
			if(r == -1) break;
		}
	}
	pthread_cleanup_pop(1);
}

void *network_pthread( void *arg )
{
	int sock_fd;
	int client_fd;
	int len;
	struct sockaddr_in serv_addr, client_addr;
	struct timeval ttime = {1, 0};
	int nzero = 0;
	int n_1k  = 1000;
	int n_set  = 1;

net_start:
	sock_fd = socket( AF_INET, SOCK_STREAM, 0 );
	if( sock_fd == -1 ) {
		printf("Create socket error.\n");
		sleep(1);
		goto net_start;
	}

	/* Send timeout: 1s. */ 
	setsockopt(sock_fd, SOL_SOCKET, SO_SNDTIMEO,  (void *)&ttime, sizeof(ttime));
	/* Send buffer is set to zero. */
	setsockopt(sock_fd, SOL_SOCKET, SO_SNDBUF,    (void *)&nzero, sizeof(int));
	/* Port can be reused. */
	setsockopt(sock_fd, SOL_SOCKET, SO_REUSEADDR, (void *)&n_set, sizeof(int));
	
	bzero( &serv_addr, sizeof(serv_addr));
	serv_addr.sin_family      = AF_INET;
	serv_addr.sin_port        = htons(NETPORT);
	serv_addr.sin_addr.s_addr = INADDR_ANY;
	bzero( &(serv_addr.sin_zero), 8 );

	if( bind(sock_fd, (struct sockaddr *)&serv_addr, sizeof (serv_addr))
			!= 0 ) {
		printf("Bind address fail.\n");
		close(sock_fd);
		sleep(1);
		goto net_start;
	}
	if( listen(sock_fd, 5) !=0 ) {
		printf("Listen socket error.\n");
		close(sock_fd);
		sleep(1);
		goto net_start;
	}
	
	while(1) {
		len = sizeof(client_addr);
		printf("Waiting connection request...\n");

		do {
			client_fd = accept(sock_fd,
					(struct sockaddr *)&client_addr, &len);
			if( client_fd <=0 ) {
				printf("Accept error. Retry...");
				sleep(1);
			}
		} while(client_fd <= 0 );
		printf("Connection created.\n");

		/* Send part. In an independent pthread. */
		pthread_create(&pid_send, NULL, send_pthread, &client_fd );
		issending = 1;
		pthread_join(pid_send, NULL);
	}
}

void *sensor_pthread(void *buffer)
{
	int i;
	int n;
	unsigned short addr;
	unsigned char  value;
	unsigned char  data_flag[3];
	signed   short data[3];
	unsigned short *buf[BUFNUM];
	struct tm *p;	//time
	time_t timep;
	struct timeval time_now;

	/* Get pointer of every buffer. */
	for( i=0 ; i<BUFNUM ; i++ )
	{
		buf[i] = (short *)(buffer + bufsize * i);
	}

	open_gpio();
	qf_port_init();

start:
	pthread_cleanup_push(backtoconfig, NULL);
	printf("Start...\n");

	/******************************/
	//1. In CONFIGURE mode
	//1.1 Write register
	for( n = 0 ; n < *QFImageRegisterTableSizeArray[fs_type]; n++ )
	{
		qf_configure_write(QFImageRegisterTableArray[fs_type][n].uiAddress, 
				QFImageRegisterTableArray[fs_type][n].ucValue);
	}
	printf("Write configure register ok.\n");
	
	//1.2 Varify register
	for( n = 0 ; n < *QFImageRegisterTableSizeArray[fs_type]; n++ )
	{
		addr = QFImageRegisterTableArray[fs_type][n].uiAddress;
		qf_configure_read(addr, &value);
		if( !qf_reg_isreadonly(addr) &&
			  value != QFImageRegisterTableArray[fs_type][n].ucValue) {
			printf("Varify: Error! addr = 0x%04x, value=0x%02x\n",
				 addr, value);
			goto error1;
		}
	}
	printf("Varify ok.\n");


	//2. Send run
	/*do{
		gettimeofday(&time_now, NULL);
	} while( (time_now.tv_usec/1000%1000) );
	*/

	for( n = 0 ; n < gTotalSendRunCommands ; n++ )
	{
		qf_configure_write(QFRunRegisterTable[n].uiAddress,
				QFRunRegisterTable[n].ucValue);
	}
	printf("In RUN mode.\n");
	
	sleep(1);

	//3. In RUN mode
	//   The function should repeat until system error.
	//   Watchdog can be added here. (run every 1 second.)
	while(1) {
	    for(i=0; i < 5000/fs[fs_type] ; i++) {

		gettimeofday(&time_now, NULL);
		p = localtime(&time_now.tv_sec);

		for( n = 0 ; n < fs[fs_type] ; n++ )
		{
			qf_run_mode(0x00, 0x00, data_flag, (unsigned short *)data);

			//printf("%d\t%d\t%d\t%d\n", n, data[0], data[1], data[2]);
			// Altogether get 10 bytes everytime.
			*(unsigned short *)(buf[i%2]+5*n+0) = node_id;
			*(unsigned short *)(buf[i%2]+5*n+1) = i*fs[fs_type] + n;
			*(  signed short *)(buf[i%2]+5*n+2) = data[0];
			*(  signed short *)(buf[i%2]+5*n+3) = data[1];
			*(  signed short *)(buf[i%2]+5*n+4) = data[2];

			/*
			// Flag verify should be added here.
			if( data_flag[0] != 0x10 )
				printf("RUN: channel X flag error. ");
				printf("flag: %02x\n", data_flag[0]);
			if( data_flag[1] != 0x30 )
				printf("RUN: channel Y flag error. ");
				printf("flag: %02x\n", data_flag[1]);
			if( data_flag[2] != 0x50 )
				printf("RUN: channel Z flag error. ");
				printf("flag: %02x\n", data_flag[2]);
			*/
		}
		
		/* Frame End: Time */
		sprintf( (char *)buf[i%2] + 10 * fs[fs_type], 
				"%c%c%c%c%c%c%c%c%c%c",
				-100+p->tm_year, 1+p->tm_mon, p->tm_mday,
				p->tm_hour, p->tm_min, p->tm_sec,
				time_now.tv_usec/1000%256, time_now.tv_usec/1000/256,
				time_now.tv_usec%1000%256, time_now.tv_usec%1000/256 );
		bzero( (char *)buf[i%2] + 10*fs[fs_type] +10, 30);

		can_read_buf = (char *)buf[i%2];
		if(issending) pthread_kill(pid_send, SIGUSR1);
	    }
	}
	pthread_cleanup_pop(1);

	//4. Send configure
	//   Actually the following should NOT execute.
	sleep(1);
	qf_run_mode(0x04, 0x00, data_flag, (unsigned short *)data);
	printf("Back to CONFIGURE mode.\n");


	/******************************/
	close_gpio();
	return 0;

error1:	//In configure mode
	qf_configure_write(0x0002, 0x0f);	//reset
	//qf_reset();
	printf("Reset Chip!\n\n");
	sleep(1);
	goto start;

error2:	//In run mode
	sleep(1);
	goto start;
	
}

int main(int argc, char *argv[])
{
	unsigned short get_node_id();

	char *buf;

	pthread_t pid0;
	pthread_t pid1;

	can_read_buf = NULL;

	node_id = get_node_id();
	fs_type = get_fs_type();
	printf("[NodeID] %d\n[Sample Frequency]%d\n", node_id,
			fs[fs_type]);

	bufsize = fs[fs_type] * 10 + FRAMEEND;
	buf = (char *)malloc( bufsize * BUFNUM );


	pthread_create(&pid0, NULL, network_pthread, NULL);
	pthread_create(&pid1, NULL, sensor_pthread, buf);

	while(1) {
		sleep(10);//do nothing
	}

	pthread_join(pid0, NULL);
	pthread_join(pid1, NULL);
	return 0;
}




